import yaml
from PIL import Image, ImageDraw

# 定义图像和大矩形
image_size = (384, 384)
background_color = 192
large_rect_size = 128

# 创建灰色画布
image = Image.new("L", image_size, background_color)
draw = ImageDraw.Draw(image)

# 大矩形的位置
large_rect_left = (image_size[0] - large_rect_size) // 2
large_rect_top = (image_size[1] - large_rect_size) // 2
large_rect_coords = (
    large_rect_left, large_rect_top,
    large_rect_left + large_rect_size, large_rect_top + large_rect_size
)
draw.rectangle(large_rect_coords, outline=0, width=2, fill=255)

# 读取坐标点
with open(r"C:\Users\17963\Desktop\ros_workspace\selected_points.txt", "r") as file:
    points = [tuple(map(float, line.strip().split(","))) for line in file]

# 经纬度范围
longitudes = [p[0] for p in points]
latitudes = [p[1] for p in points]

min_lon, max_lon = min(longitudes), max(longitudes)
min_lat, max_lat = min(latitudes), max(latitudes)

# 计算经纬度跨度，并统一比例尺
lon_span = max_lon - min_lon
lat_span = max_lat - min_lat
scale = max(lon_span, lat_span)  # 使用更大的跨度作为统一比例尺

# 计算每像素对应的经纬度跨度
pixel_per_unit = large_rect_size / scale  # 一个单位经纬度占用多少像素
gps_per_pixel = 1 / pixel_per_unit  # 一个像素对应的经纬度

# 大矩形左上角对应的经纬度
top_left_lon = min_lon
top_left_lat = max_lat

# 映射到大矩形内部
mapped_points = []
for lon, lat in points:
    # 经度归一化
    norm_x = (lon - min_lon) / scale
    # 纬度归一化并颠倒
    norm_y = (max_lat - lat) / scale

    # 映射到大矩形
    x = large_rect_left + int(norm_x * large_rect_size)
    y = large_rect_top + int(norm_y * large_rect_size)
    mapped_points.append((x, y))

# 绘制闭合线条
draw.polygon(mapped_points, outline=0)  # 闭合的线条，黑色边框

# 保存图像
image.save("output_with_polygon.pgm")
print("图像已保存为output_with_polygon.pgm")

# 生成 YAML 配置文件
config_data = {
    "large_rectangle": {
        "top_left": {
            "longitude": float(top_left_lon),
            "latitude": float(top_left_lat),
        },
        "pixel_per_unit": float(pixel_per_unit),  # 确保科学计数法格式为人类可读
        "gps_per_pixel": float(gps_per_pixel),    # 同上
    },
    "image_size": list(image_size),  # 转换为列表
    "background_color": background_color,
    "large_rect_size": large_rect_size,
}

with open("config.yaml", "w") as yaml_file:
    yaml.dump(config_data, yaml_file, default_flow_style=False)
print("配置文件已保存为config.yaml")
